import sys
import threading
from threading import Thread,Event,Lock
import time
import os
os.environ["OPENCV_VIDEOIO_MSMF_ENABLE_HW_TRANSFORMS"] = "0"
import cv2
from PyQt5 import QtCore, QtGui, QtWidgets
from PyQt5.QtWidgets import QFileDialog,QWidget
from PyQt5.QtCore import Qt , QPoint
from PyQt5.QtGui import QCloseEvent
import numpy as np
import json
import math
import serial
import struct
import cauto
from multiprocessing import Pool,Process,Value,Array,Queue
from supercontrol import log
import SuperGluePretrainedNetwork.sg
Q=Queue(1)
class clickableQlabel(QtWidgets.QLabel):
    def __init__(self, parent=None):
        super(clickableQlabel, self).__init__(parent)
        self.point_list=[]
        try:
            with open('pointListLog.txt','r') as f:
                self.point_list=json.load(f)
        except:
            pass
    def mousePressEvent(self, event) :   
    # 鼠标左键按下
        if event.button() == Qt.LeftButton :
            self.lastPoint = event.pos()
            self.endPoint = self.lastPoint
            #print(event.pos())
            self.point_list.append((event.pos().x(), event.pos().y()))
            with open('pointListLog.txt','w') as f:
                json.dump(self.point_list,f)
arucoDict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_4X4_50)
arucoParams = cv2.aruco.DetectorParameters_create()
arucoParams.adaptiveThreshConstant=7
arucoParams.adaptiveThreshWinSizeStep=5
# arucoParams.adaptiveThreshWinSizeMax=40
# arucoParams.aprilTagMaxNmaxima=40
# arucoParams.aprilTagCriticalRad=0
# arucoParams.aprilTagMinClusterPixels=0
arucoParams.maxErroneousBitsInBorderRate=0.4
# arucoParams.errorCorrectionRate=0.8
arucoParams.perspectiveRemoveIgnoredMarginPerCell=0.3
arucoParams.polygonalApproxAccuracyRate=0.01
arucoParams.maxMarkerPerimeterRate =4
arucoParams.minCornerDistanceRate=0
arucoParams.minDistanceToBorder=0
arucoParams.minMarkerDistanceRate=0.005 #重要
arucoParams.aprilTagMaxLineFitMse=0.00


suckerOffset={1:[-40,415],2:[415,40],3:[33,-420],4:[98,415],5:[-92,-410]}
from supercontrol import Move
from supercontrol import controlRotate
from supercontrol import controlSuckerOn
from supercontrol import controlSuckerOff
from supercontrol import controlSteeringEngineDown
from supercontrol import Calculate
def run(para,Q):
    log.info(f"run:{para}")
    for i in para:
        if i[0]=='rotate':
            controlRotate(**i[1],Q=Q)
        if i[0]=='move':
            Move(**i[1],Q=Q)
        if i[0]=='controlSuckerOn':
            controlSuckerOn(**i[1])
        if i[0]=='controlSuckerOff':
            controlSuckerOff(**i[1])
        if i[0]=='controlSteeringEngineDown':
            controlSteeringEngineDown(**i[1])
        if i[0]=='Calculate':
            Calculate(**i[1],Q=Q)
def slipshot(obj,rvet,tvet,cameraMatrix,distCoeffs):
    print(obj.shape)
    try:
        re,jac=cv2.projectPoints(obj,rvet,tvet,cameraMatrix,distCoeffs)
        shot=re.reshape(-1,2)
        shot=np.round(shot).astype('int32')
        # print(shot)
    except Exception as e:
        print('error',e)
    return shot
fuelPos3D=np.array([
            [3925-5,2250-5,310],
            [3925-5,2400-5,310],
            [3925-5,2550-5,310]
        ]).astype('float32')
mineralPos3D=np.array([ #officially
            [2850-5,75+5,110],
            [3000-5,75+5,110],
            [3150-5,75+5,110],
            [3300-5,75+5,110],
            [3450-5,75+5,110],
            [2850-5,1675+5,310],
            [3000-5,1675+5,310],
            [3150-5,1675+5,310],
            [3300-5,1675+5,310],
            [3450-5,1675+5,310],
            [2850-5,3125-5,310],
            [3000-5,3125-5,310],
            [3150-5,3125-5,310],
            [3300-5,3125-5,310],
            [3450-5,3125-5,310],
        ]).astype('float32')
dMap=cv2.imread('map.png')
class CameraThread(Thread):
    def __init__(self, kill_event, src = 0, width = 1920, height = 1080):
        self.kill_event = kill_event
        
        self.stream = cv2.VideoCapture(src)
        self.stream.set(cv2.CAP_PROP_FRAME_HEIGHT, height)
        self.stream.set(cv2.CAP_PROP_FRAME_WIDTH, width)
        self.stream.set(cv2.CAP_PROP_FPS, 60)
        (self.grabbed, self.frame) = self.stream.read()
        self.read_lock = Lock()

        Thread.__init__(self, args = kill_event)

    def update(self):
        (grabbed, frame) = self.stream.read()
        self.read_lock.acquire()
        self.grabbed, self.frame = grabbed, frame
        self.read_lock.release()

    def read(self):
        self.read_lock.acquire()
        frame = self.frame.copy()
        self.read_lock.release()
        return frame

    def run(self):
        while not self.kill_event.is_set():
            self.update()


class Ui_Detect(QWidget):
    signal=QtCore.pyqtSignal()
    isCalced=False
    def __init__(self):
        super().__init__()
        self.isCapture = False

        try:
            data=np.load('data.npz')
            self.cameraMatrix=data['arr_0']
            self.distCoeffs=data['arr_1']
            self.rvet=data['arr_2']
            self.tvet=data['arr_3']
            self.objp=data['arr_4']
            self.objp2Dshot=data['arr_5']
            self.objpMap=data['arr_6']
            self.isCalced=True
            log.info('读取成功')
        except:
            self.isCalced=False
            log.info('读取失败')

        self.signal.connect(self.open)

        self.calc=QtWidgets.QPushButton(self)
        self.calc.clicked.connect(self.initMap)
        self.calc.setText("计算")
        self.calc.move(1920,30)
        self.calc.show()

        self.calc_mineral=QtWidgets.QPushButton(self)
        self.calc_mineral.clicked.connect(self.detectMineralColor)
        self.calc_mineral.setText("分析矿物颜色")
        self.calc_mineral.move(1920,60)
        self.calc_mineral.show()

        self.calc_fuel=QtWidgets.QPushButton(self)
        self.calc_fuel.clicked.connect(self.detectFuelColor)
        self.calc_fuel.setText("分析燃料矿颜色")
        self.calc_fuel.move(1920,90)
        self.calc_fuel.show()

        self.calc_meteorites=QtWidgets.QPushButton(self)
        self.calc_meteorites.clicked.connect(self.detectMeteorites)
        self.calc_meteorites.setText("分析陨石")
        self.calc_meteorites.move(1920,120)
        self.calc_meteorites.show()

        self.calc_corner=QtWidgets.QPushButton(self)
        self.calc_corner.clicked.connect(self.cornerDetect)
        self.calc_corner.setText("角点")
        self.calc_corner.move(1920,150)
        self.calc_corner.show()

        self.con=QtWidgets.QPushButton(self)
        self.con.clicked.connect(self.vehicleControl)
        self.con.setText("开始控制车辆")
        self.con.move(1920,180)
        self.con.show()

        self.label = clickableQlabel(self)
        self.label.setText("")
        self.label.setObjectName("label")
        self.label.move(0,0)
        self.label.show()
        self.label.resize(1920,1080)
        frame=cv2.imread('back.png')
        img = QtGui.QImage(frame.data, frame.shape[1], frame.shape[0], QtGui.QImage.Format_BGR888)
        self.label.setPixmap(QtGui.QPixmap.fromImage(img))
        #self.signal.emit()
        self.closed=False
        self.move(0,0)
        self.show()
        self.open()
        
        # if len(self.label.point_list)==6:
        #     self.initMap()

    def draw_point_list(self,img,list):
        for (id,point) in enumerate(list):
            cv2.circle(img, (round(point[0]), round(point[1])), 2, (0, 255, 255), 2)
            #cv2.putText(img,str(id),(point[0], point[1]),cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2)
        return img
    def closeEvent(self,event: QCloseEvent):
        log.info("主窗口已关闭")
        self.closed=True
    def open(self):
        log.info("打开摄像头")
        # cap = cv2.VideoCapture(0)
        # cap.set(cv2.CAP_PROP_FRAME_HEIGHT,1080)
        # cap.set(cv2.CAP_PROP_FRAME_WIDTH,1920)
        # cap.set(cv2.CAP_PROP_FPS, 60)
        kill_event = Event()
        cam = CameraThread(kill_event)
        cam.start()
        # while cap.isOpened() and self.closed==False:
        while self.closed==False:
            # ti=time.time()
            # ret, frame = cap.read()
            frame=cam.read()
            self.image=frame.copy()
            # print(time.time()-ti)
            if self.isCalced:
                re=self.detectArucoPos()
                # print(time.time()-ti)
                # if re!=None:
                #     pos,angle=re
                #     cv2.circle(dMap, (round(pos[0]/2), round(pos[1]/2)), 2, (0, 0, 0), 2)
                #     cv2.imshow('dMap',dMap)
                #     # cv2.moveWindow('dMap',2560,0)
                cv2.waitKey(1)
                # print(time.time()-ti)
                # log.info(f'detectAruco:{time.time()-ti}sec')
            else:
                i,o=self.matcher.match(self.image)
                for pos in i:
                    cv2.circle(frame, (round(pos[0]), round(pos[1])), 1, (255,255,0), 2)
            # hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
            # mask = cv2.inRange(hsv, np.array([0,0,220]), np.array([180,255,255]))
            # # # cv2.imshow('mask',mask)
            # # # cv2.waitKey(1)
            # (corners, ids, rejected) = cv2.aruco.detectMarkers(mask, arucoDict,parameters=arucoParams)
            # print("rej",rejected)
            # #print('corners',corners)
            # frame=cv2.aruco.drawDetectedMarkers(frame,corners,ids)
            img = QtGui.QImage(frame.data, frame.shape[1], frame.shape[0], QtGui.QImage.Format_BGR888)
            self.label.setPixmap(QtGui.QPixmap.fromImage(img))
                # self.label.resize(1920,1080)
            # print(time.time()-ti)
    def initMap(self):
        '''init'''
        self.isCalced=True
        self.matcher=SuperGluePretrainedNetwork.sg.sgmatch()
        imagePoints,objectPoints=self.matcher.match(self.image)
        imagePoints=np.array(imagePoints,dtype='float32')
        objectPoints=np.array(objectPoints,dtype='float32')
        # self.cameraMatrix=np.array( [[894.348826   ,  0,         936.44500723],
        #                         [  0,         896.42857334, 558.08348431],
        #                         [  0,           0,           1,        ]])
        # self.cameraMatrix=np.array( [[1.04060143e+03 ,0.00000000e+00 ,1.00758326e+03],
        #                             [0.00000000e+00 ,1.03991261e+03 ,4.94579296e+02],
        #                             [0.00000000e+00 ,0.00000000e+00 ,1.00000000e+00]])
        self.distCoeffs=np.array([0,0,0,0,0]).astype('float32')
        self.cameraMatrix=np.array([[1.03821817e+03 ,0.00000000e+00 ,1.00618003e+03],
                                    [0.00000000e+00 ,1.03840613e+03 ,4.94445799e+02],
                                    [0.00000000e+00 ,0.00000000e+00 ,1.00000000e+00]])
        self.distCoeffs=np.array([[-0.03792993  ,0.1240955   ,0.00080589 ,-0.00014446 ,-0.10270177]],dtype='float32')
        retval,self.rvet,self.tvet=cv2.solvePnP(objectPoints,imagePoints,self.cameraMatrix,self.distCoeffs)
        log.info(f'rvet{self.rvet}')
        log.info(f'tvet{self.tvet}')
        w=4000
        h=4000
        interval=2
        vehicleHight=370
        # vehicleHight=0
        ti=time.time()
        # self.objp=np.array([(i,j,200 if (i>=1600 and j>=1600) else 0) for i in range(0,w,interval) for j in range(0,h,interval)]).astype('float32')
        self.objp=cauto.build(vehicleHight,interval)# 车体高度
        # print('objp',self.objp)
        print(f"{time.time()-ti} sec")
        # re,jac=cv2.projectPoints(self.objp,self.rvet,self.tvet,self.cameraMatrix,self.distCoeffs)
        num=10
        pool = Pool(processes=num) # 子进程数
        results = []
        start = time.time()
        for n in range(num):
            result = pool.apply_async(slipshot, (self.objp[len(self.objp)//num*n:len(self.objp)//num*(n+1),:],self.rvet,self.tvet,self.cameraMatrix,self.distCoeffs))
            results.append(result)
        pool.close()
        pool.join()
        self.objp2Dshot=np.zeros((0,2),dtype='int32')
        for res in results:
            self.objp2Dshot=np.append(self.objp2Dshot,res.get(),axis=0)

        print(f"calc:{time.time()-ti} sec")
        self.objpMap=cauto.fill(self.objp2Dshot)
        print(f"fill:{time.time()-ti} sec")
        img=self.image
        for pos in imagePoints:
            cv2.circle(img, (round(pos[0]), round(pos[1])), 1, (255,255,0), 2)
        for (id,point) in enumerate(self.objp2Dshot[::100]):
            cv2.circle(img, (round(point[0]), round(point[1])), 2, (0, 255, 0), 2)
        cv2.imshow('img',img)
        cv2.waitKey(3000)
        cv2.destroyAllWindows()
        np.savez('data',self.cameraMatrix,self.distCoeffs,self.rvet,self.tvet,self.objp,self.objp2Dshot,self.objpMap)
        log.info('保存完成')
        
    def get3Dpos(self,pos2D):
        return self.objp[self.objpMap[round(pos2D[1]),round(pos2D[0])]]
        # npos2D=np.repeat([pos2D],len(self.objp),axis=0)
        # nnorm=np.linalg.norm(self.objp2Dshot-npos2D,ord=1,axis=1,keepdims=False)
        # index=nnorm.argmin()
        # return self.objp[index]
    def detectMineralColor(self):
        re,jac=cv2.projectPoints(mineralPos3D,self.rvet,self.tvet,self.cameraMatrix,self.distCoeffs)
        mineralPos2D=re.reshape(-1,2)
        #print("mineralPos2D:\n",mineralPos2D)
        hsv=cv2.cvtColor(self.image,cv2.COLOR_BGR2HSV)
        # print(len(hsv),len(hsv[0]))
        # cv2.imwrite('img.png',self.image)
        H,S,V=cv2.split(hsv)
        # B,G,R=cv2.split(self.image)
        mineralH=[H[round(i[1]),round(i[0])] for i in mineralPos2D]
        print('mineralH',mineralH)
        sorted_mineralH=sorted(mineralH)
        # mineralHThreshold=sorted_mineralH[6]
        mineralHThreshold=60
        mineralColor=['poor' if i>=mineralHThreshold else 'rich' for i in mineralH]
        log.info(f'mineralColor:{mineralColor}')
        richMineralPos=[[],[],[]]
        for i in range(0,5):
            if mineralColor[i]=='rich':
                richMineralPos[0].append(i)
        for i in range(5,10):
            if mineralColor[i]=='rich':
                richMineralPos[1].append(i)
        for i in range(10,15):
            if mineralColor[i]=='rich':
                richMineralPos[2].append(i)
        log.info(richMineralPos)
        img=self.image
        for (id,point) in enumerate(mineralPos2D):
            cv2.circle(img, (round(point[0]), round(point[1])), 2, (255, 255, 0), 2)
            # print(B[round(point[1]), round(point[0])],G[round(point[1]), round(point[0])],R[round(point[1]), round(point[0])])
        cv2.imshow('img',img)
        cv2.waitKey(1000)
        cv2.destroyAllWindows()
        return richMineralPos
    def detectFuelColor(self):
        re,jac=cv2.projectPoints(fuelPos3D,self.rvet,self.tvet,self.cameraMatrix,self.distCoeffs)
        fuelPos2D=re.reshape(-1,2)
        #print("mineralPos2D:\n",mineralPos2D)
        hsv=cv2.cvtColor(self.image,cv2.COLOR_BGR2HSV)
        # cv2.imwrite('img.png',self.image)
        H,S,V=cv2.split(hsv)
        # B,G,R=cv2.split(self.image)
        fuelH=[H[round(i[1]),round(i[0])] for i in fuelPos2D]
        print('fuelH',fuelH)
        sorted_fuelH=sorted(fuelH)
        fuelHThreshold=sorted_fuelH[1]
        fuelColor=['occupied' if i>=fuelHThreshold else 'empty' for i in fuelH]
        log.info(f'fuelColor:{fuelColor}')
        fuelPos=[[],[]]
        for i in range(0,3):
            if fuelColor[i]=='occupied':
                fuelPos[1].append(i)
        # for i in range(3,6):
        #     if fuelColor[i]=='occupied':
        #         fuelPos[1].append(i)
        log.info(fuelPos)
        img=self.image
        for (id,point) in enumerate(fuelPos2D):
            cv2.circle(img, (round(point[0]), round(point[1])), 2, (255, 255, 0), 2)
            #print(B[round(point[1]), round(point[0])],G[round(point[1]), round(point[0])],R[round(point[1]), round(point[0])])
        cv2.imshow('img',img)
        cv2.waitKey(1000)
        cv2.destroyAllWindows()
        return fuelPos
        
    def detectMeteorites(self):
        img=self.image
        gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
        corners = cv2.goodFeaturesToTrack(gray, 300, 0.05, 10)
        print("len",len(corners))
        re=[[0,0],[0,0]]
        for pt in corners:
            #print(pt)
            x2D = np.int32(pt[0][0])
            y2D = np.int32(pt[0][1])
            x,y,z=self.get3Dpos((x2D,y2D))
            # print(x,y,z)
            if 0<x<2200 and 0<y<=800:
                re[0][0]+=1
                cv2.circle(img, (x2D, y2D), 5, (255, 0, 0), 2)
            elif 0<x<2200 and 800<y<=1600:
                re[0][1]+=1
                cv2.circle(img, (x2D, y2D), 5, (0, 255, 0), 2)
            elif 2200<=x<=3000 and 0<y<=800:
                re[1][0]+=1
                cv2.circle(img, (x2D, y2D), 5, (0, 0, 255), 2)
            elif 2200<=x<=3000 and 800<y<=1600:
                re[1][1]+=1
                cv2.circle(img, (x2D, y2D), 5, (255, 255, 0), 2)
            else:
                cv2.circle(img, (x2D, y2D), 5, (0, 255, 255), 2)
        log.info(re)
        # cv2.imshow("img",img)
        # cv2.waitKey(1)
        return [[True,False] if re[0][0]>re[0][1] else [False,True],[True,False] if re[1][0]>re[1][1] else [False,True]]
    def cornerDetect(self):
        img=self.image
        gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
        corners = cv2.goodFeaturesToTrack(gray, 100, 0.05, 10)
        print(len(corners))
        for pt in corners:
            print(pt)
            b = np.random.random_integers(0, 256)
            g = np.random.random_integers(0, 256)
            r = np.random.random_integers(0, 256)
            x = np.int32(pt[0][0])
            y = np.int32(pt[0][1])
            cv2.circle(img, (x, y), 5, (int(b), int(g), int(r)), 2)
        cv2.imshow("img",img)
        cv2.waitKey(1)
    def detectArucoPos(self):
        ti=time.time()
        img=self.image
        hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
        mask = cv2.inRange(hsv, np.array([0,0,220]), np.array([180,255,255]))
        # cv2.imshow("mask",mask)
        # cv2.waitKey(0)
        # print(time.time()-ti)
        (corners, ids, rejected) = cv2.aruco.detectMarkers(mask, arucoDict,parameters=arucoParams)
        # print(time.time()-ti)
        global Q
        if len(corners)!=0:
            maxPerimeter=-1
            maxPos=-1
            for i,id in zip(corners,ids):
                if id !=0:
                    continue
                perimeter=math.sqrt((i[0,0,0]-i[0,1,0])**2+(i[0,0,1]-i[0,1,1])**2)+math.sqrt((i[0,1,0]-i[0,2,0])**2+(i[0,1,1]-i[0,2,1])**2)+math.sqrt((i[0,2,0]-i[0,3,0])**2+(i[0,2,1]-i[0,3,1])**2)+math.sqrt((i[0,3,0]-i[0,0,0])**2+(i[0,3,1]-i[0,0,1])**2)
                if perimeter>maxPerimeter:
                    maxPerimeter = perimeter
                    maxPos=i
            if maxPerimeter!=-1:
                i=maxPos
                point0=self.get3Dpos((i[0,0,0],i[0,0,1]))
                point1=self.get3Dpos((i[0,1,0],i[0,1,1]))
                point2=self.get3Dpos((i[0,2,0],i[0,2,1]))
                point3=self.get3Dpos((i[0,3,0],i[0,3,1]))

                if point0[2]!=point1[2] or point0[2]!=point2[2] or point0[2]!=point3[2]:
                    log.info("交界处,识别失败")
                    try:
                        Q.put_nowait([-1926,[-1926,-1926]])
                    except:
                        pass
                    return None

                # log.info(f'{point0[2]} {point1[2]} {point2[2]} {point3[2]}')

                alpha=math.atan2(point0[1]-point3[1],point0[0]-point3[0])# 都是弧度制 #可以再平均

                # img=cv2.aruco.drawDetectedMarkers(img,[i],np.array([0]))
                # cv2.imshow("img",img)

                pos=((point0[0]+point1[0]+point2[0]+point3[0])/4,(point0[1]+point1[1]+point2[1]+point3[1])/4)
                # print(pos[0],pos[1])
                angle=alpha
                # print(time.time()-ti)
                log.info(f'pos:{pos},angle:{angle}')
                try:
                    Q.put_nowait([angle,pos])
                except:
                    pass
                    # print(time.time())
                return pos,alpha
        log.info("识别失败")
        try:
            Q.put_nowait([-1926,[-1926,-1926]])
        except:
            pass
            # print(time.time())
        # cv2.imwrite("failimg.png",img)#记得删
        # cv2.imwrite("failmask.png",mask)
        return None
    def generateMeteoritesRegionParameters(self):
        Meteorites=self.detectMeteorites()
        vpa=([50,300,500,1e18],[100,200,300,300])
        if Meteorites[0][0]==Meteorites[1][0]:
            if Meteorites[0][0]:
                para=[('Calculate',{'endPos':(350,1100),'targetAngle':math.pi/2}),('Calculate',{'endPos':(2300,1100),'targetAngle':math.pi/2})]
            else:
                para=[('Calculate',{'endPos':(350,500),'targetAngle':math.pi/2}),('Calculate',{'endPos':(2300,500),'targetAngle':math.pi/2})]
        else:
            if Meteorites[0][0]:
                para0=[('Calculate',{'endPos':(350,1100),'targetAngle':math.pi/2,'vpara':vpa}),('Calculate',{'endPos':(1500,1100),'targetAngle':math.pi/2,'vpara':vpa})]
            else:
                para0=[('Calculate',{'endPos':(350,500),'targetAngle':math.pi/2,'vpara':vpa}),('Calculate',{'endPos':(1500,500),'targetAngle':math.pi/2,'vpara':vpa})]

            if Meteorites[1][0]:
                para1=[('Calculate',{'endPos':(1700,1100),'targetAngle':math.pi/2,'vpara':vpa}),('Calculate',{'endPos':(2300,1100),'targetAngle':math.pi/2,'vpara':vpa})]
            else:
                para1=[('Calculate',{'endPos':(1700,500),'targetAngle':math.pi/2,'vpara':vpa}),('Calculate',{'endPos':(2300,500),'targetAngle':math.pi/2,'vpara':vpa})]
            para=para0+para1
        backPara=para
        para[0][1]['vpara']=([50,300,500,1e18],[100,200,300,500])
        # log.info(Meteorites)
        # log.info(para0)
        # log.info(para1)
        return [('rotate',{'targetAngle':math.pi/2}),('Calculate',{'endPos':(350,2400),'targetAngle':math.pi/2})]+para,[('rotate',{'targetAngle':math.pi/2})]+backPara[::-1]+[('rotate',{'targetAngle':math.pi/2}),('Calculate',{'endPos':(350,2400),'targetAngle':math.pi/2})]
    def vehicleControl(self):

        # log.info("start control")
        # CE=Process(target=run, args=([('Calculate',{'endPos':(3000,800),'targetAngle':math.pi,'vpara':([50,300,500,1e18],[100,100,200,200])})],Q,))
        # CE.start()
        # return

        Minerals=self.detectMineralColor()
        Fuels=self.detectFuelColor()

        #第一轮上坡

        上坡=[('Calculate',{'endPos':(400,2500),'vpara':([50,300,500,1e18],[100,200,300,500]),'earlyStopRange':20}),
            ('rotate',{'targetAngle':0}),
            ('rotate',{'targetAngle':0}),
            ('Calculate',{'endPos':(2700,2500),"vpara":([50,400,500,1e18],[100,100,500,500]),'earlyStopRange':100})]
        # CE=Process(target=run, args=(上坡,Q,))
        # CE.start()
        # return
        微调=([50,300,500,1e18],[25,25,25,25])
        pos=mineralPos3D[Minerals[2][0]]
        矿石120=[('Calculate',{'endPos':(pos[0]-suckerOffset[1][0],pos[1]-suckerOffset[1][1]),'vpara':([50,300,500,1e18],[50,100,200,300]),'earlyStopRange':10}),
                ('rotate',{'targetAngle':0}),
                ('Calculate',{'endPos':(pos[0]-suckerOffset[1][0],pos[1]-suckerOffset[1][1]),'vpara':微调,'earlyStopRange':10,'safeStop':True}),
                ('controlSuckerOn',{"suckerNumber":1})]
        pos=mineralPos3D[Minerals[2][1]]
        矿石121=[('Calculate',{'endPos':(pos[0]-suckerOffset[4][0],pos[1]-suckerOffset[4][1]),'vpara':([50,300,500,1e18],[50,100,200,300]),'earlyStopRange':10}),
                ('rotate',{'targetAngle':0}),
                ('Calculate',{'endPos':(pos[0]-suckerOffset[4][0],pos[1]-suckerOffset[4][1]),'vpara':微调,'earlyStopRange':10,'safeStop':True}),
                ('controlSuckerOn',{"suckerNumber":4})]
        
        # 燃料矫正=[('move',{'endPos':(3450,2400)}),('rotate',{'targetAngle':0})]

        pos=fuelPos3D[Fuels[1][1]]
        燃料0=[('Calculate',{'endPos':(pos[0]-suckerOffset[2][0],pos[1]-suckerOffset[2][1]),'vpara':([50,300,500,1e18],[50,100,200,300]),'earlyStopRange':10}),
                ('rotate',{'targetAngle':0}),
                ('Calculate',{'endPos':(pos[0]-suckerOffset[2][0],pos[1]-suckerOffset[2][1]),'vpara':微调,'earlyStopRange':10,'safeStop':True}),
                ('controlSuckerOn',{"suckerNumber":2})]
        
        下坡=[('Calculate',{'endPos':(2700,2500)}),('rotate',{'targetAngle':0}),('Calculate',{'endPos':(420,2500),'vpara':([50,300,500,1e18],[100,100,300,300]),'earlyStopRange':50})]

        放矿1=[('rotate',{'targetAngle':math.pi/2}),
            ('move',{'endPos':(400,2600-suckerOffset[4][0]),'targetAngle':math.pi/2,'vpara':([50,300,500,1e18],[100,100,200,200])}),
            ('controlSuckerOff',{"suckerNumber":4}),
            ('move',{'endPos':(400,2600-suckerOffset[1][0]),'targetAngle':math.pi/2,'vpara':([50,300,500,1e18],[100,100,200,200])}),
            ('controlSuckerOff',{"suckerNumber":1})]
        放矿2=[('rotate',{'targetAngle':math.pi}),('move',{'endPos':(400,3000+suckerOffset[2][1]),'targetAngle':math.pi,'vpara':([50,300,500,1e18],[100,100,200,200])}),('controlSuckerOff',{"suckerNumber":2})]

        回正=[('rotate',{'targetAngle':0})]
        回倒=[('rotate',{'targetAngle':math.pi})]

        放舵机14=[('controlSteeringEngineDown',{"suckerNumber":1}),('controlSteeringEngineDown',{"suckerNumber":4})]
        放舵机124=[('controlSteeringEngineDown',{"suckerNumber":1}),('controlSteeringEngineDown',{"suckerNumber":2}),('controlSteeringEngineDown',{"suckerNumber":4})]
        放舵机235=[('controlSteeringEngineDown',{"suckerNumber":2}),('controlSteeringEngineDown',{"suckerNumber":3}),('controlSteeringEngineDown',{"suckerNumber":5})]    

        第一轮=回正+上坡+放舵机124+回正+回正+矿石120+矿石121+燃料0+下坡+放矿1+放矿2
        #第二轮上坡

        pos=mineralPos3D[Minerals[1][0]]
        矿石210=[('Calculate',{'endPos':(pos[0]-suckerOffset[5][0],pos[1]-suckerOffset[5][1]),'vpara':([50,300,500,1e18],[50,100,200,300]),'earlyStopRange':10}),
                ('rotate',{'targetAngle':0}),
                ('Calculate',{'endPos':(pos[0]-suckerOffset[5][0],pos[1]-suckerOffset[5][1]),'vpara':微调,'earlyStopRange':10,'safeStop':True}),
                ('controlSuckerOn',{"suckerNumber":5})]
        pos=mineralPos3D[Minerals[1][1]]
        矿石211=[('Calculate',{'endPos':(pos[0]-suckerOffset[3][0],pos[1]-suckerOffset[3][1]),'vpara':([50,300,500,1e18],[50,100,200,300]),'earlyStopRange':10}),
                ('rotate',{'targetAngle':0}),
                ('Calculate',{'endPos':(pos[0]-suckerOffset[3][0],pos[1]-suckerOffset[3][1]),'vpara':微调,'earlyStopRange':10,'safeStop':True}),
                ('controlSuckerOn',{"suckerNumber":3})]
        pos=fuelPos3D[Fuels[1][0]]
        燃料1=[('Calculate',{'endPos':(pos[0]-suckerOffset[2][0],pos[1]-suckerOffset[2][1]),'vpara':([50,300,500,1e18],[50,100,200,300]),'earlyStopRange':10}),
                ('rotate',{'targetAngle':0}),
                ('Calculate',{'endPos':(pos[0]-suckerOffset[2][0],pos[1]-suckerOffset[2][1]),'vpara':微调,'earlyStopRange':10,'safeStop':True}),
                ('controlSuckerOn',{"suckerNumber":2})]

        放矿3=[('rotate',{'targetAngle':-math.pi/2}),
            ('move',{'endPos':(400,2600+suckerOffset[5][0]),'targetAngle':-math.pi/2,'vpara':([50,300,500,1e18],[100,100,200,200])}),
            ('controlSuckerOff',{"suckerNumber":5}),
            ('move',{'endPos':(400,2600+suckerOffset[3][0]),'targetAngle':-math.pi/2,'vpara':([50,300,500,1e18],[100,100,200,200])}),
            ('controlSuckerOff',{"suckerNumber":3})]

        第二轮=回正+上坡+放舵机235+回正+回正+矿石210+矿石211+燃料1+下坡+放矿3+放矿2
        #第三轮过陨石


        过陨石,回陨石=self.generateMeteoritesRegionParameters()
        pos=mineralPos3D[Minerals[0][0]]
        矿石300=[('Calculate',{'endPos':(pos[0]+suckerOffset[4][0],pos[1]+suckerOffset[4][1]),'vpara':([50,300,500,1e18],[50,100,200,200]),'earlyStopRange':10,'targetAngle':math.pi}),
                ('rotate',{'targetAngle':math.pi}),
                ('Calculate',{'endPos':(pos[0]+suckerOffset[4][0],pos[1]+suckerOffset[4][1]),'vpara':微调,'earlyStopRange':10,'safeStop':True,'targetAngle':math.pi}),
                ('controlSuckerOn',{"suckerNumber":4,'back':-math.pi/2})]
        pos=mineralPos3D[Minerals[0][1]]
        矿石301=[('Calculate',{'endPos':(pos[0]+suckerOffset[1][0],pos[1]+suckerOffset[1][1]),'vpara':([50,300,500,1e18],[50,100,200,300]),'earlyStopRange':10,'targetAngle':math.pi}),
                ('rotate',{'targetAngle':math.pi}),
                ('Calculate',{'endPos':(pos[0]+suckerOffset[1][0],pos[1]+suckerOffset[1][1]),'vpara':微调,'earlyStopRange':10,'safeStop':True,'targetAngle':math.pi}),
                ('controlSuckerOn',{"suckerNumber":1,'back':-math.pi/2})]

        第三轮=过陨石+回倒+回倒+放舵机14+矿石300+回倒+矿石301+回陨石+放矿1

        # CE=Process(target=run, args=([('move',{'endPos':(2500,800)})],mangle,mpos,))
        # CE=Process(target=run, args=([('rotate',{'targetAngle':0})],mangle,mpos,))
        # CE=Process(target=run, args=([('controlSuckerOn',{"suckerNumber":2})],mangle,mpos,))
        log.info("start control")
        # CE=Process(target=run, args=(第一轮+第三轮+第二轮,Q,))
        CE=Process(target=run, args=(第一轮+第三轮+第二轮+第三轮+第二轮+第三轮+第二轮,Q,))
        CE.start()
          
if __name__ == "__main__":
    app = QtWidgets.QApplication(sys.argv)
    window = QtWidgets.QWidget()
    ui = Ui_Detect()
    # ui.show()
    # ui.signal.emit()
